//----------------------------------------------------------------------
/*!\file
 *
 * \author  
 * \date   
 *
 */
//----------------------------------------------------------------------
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>

#include <string.h>
#include <stdio.h>
#include <sstream>
#include <fstream>
#include <vector>
#include <math.h>
#include <unistd.h>
#define SERVER_HOST "10.7.5.101"
#define SERVER_PORT 8899
// #define RealRobot 
using namespace std;

class HardwareInterface : public hardware_interface::RobotHW
{
public:
  HardwareInterface();
  ~HardwareInterface();
  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
  virtual void read(const ros::Time& time, const ros::Duration& period) override;
  virtual void write(const ros::Time& time, const ros::Duration& period) override;

  bool checkControllerClaims(const std::set<std::string>& claimed_resources);
  
  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                  const std::list<hardware_interface::ControllerInfo>& stop_list);
  void getJointStatus(vector<double> &JP);
  bool shouldResetControllers();

private:
  /*!
   * \brief Hardware interface to support reading the state of an array of named joints, each of which 
   * has some position, velocity, and effort (force or torque).
   */
  hardware_interface::JointStateInterface js_interface_;
  /*!
   * \brief for commanding position-based joints.
   */
  hardware_interface::PositionJointInterface pj_interface_;
  bool position_controller_running_;
  bool controller_reset_necessary_;
  bool useRealRobot;

  int ret;

  std::vector<std::string>  joint_names_;

  std::vector<double> cmd;
  std::vector<double> pos;
  std::vector<double> vel;
  std::vector<double> eff;
};
